'''
####
Date : 2025-1-8
Notes: 通讯
此脚本主要功能:
1. 新建线程与ZBot客户端进行通讯-数据交互
2. 监控相关寄存器值,做出指定动作。如:产品中的KT产线,台面的按钮（急停等）操作

####
'''

# -*- coding: utf-8 -*-
import logging
import os
import socket
import select
import struct
import time
import errno
import queue
from queue import Queue
import threading
import modbus_tk.defines as cst
import modbus_tk.modbus_tcp as modbus_tcp

from threading import Thread
# from PyQt5.QtCore import QTimer

from processModbusTCP__ import process_Master
# from processModbusRTU_ import rtu_Master
# from processCommunicate_ import VSR_Server

# # C++编写的ModbusTCP服务端
# from TCPmodbus_server import Modbus_Server
# # C++编写的ModbusTCP客户端
# from TCPmodbus_wrapper import ModbusMaster
# import config
### 日志文件
robot_logger=logging.getLogger("ZMove")
robot_logger.setLevel(logging.DEBUG)
### 默认显示
os.environ['DISPLAY'] = ':0'

# process_master = process_Master()
# rtu_master = rtu_Master()

POSITION_THRESHOLD = 0.1  # 根据实际机械精度调整

from _path_planner import LinearPlanner
from _exceptions import RobotNotConnectedError, MotionPlanningError
import _database
CL = _database.LocalDatabase()


class VSRControler_Server():
    def __init__(self, simulate=False):
        self.simulate = simulate

        self.host = "0.0.0.0"
        self.port = 502
        self.modbus_tcp_init()
        self.modbusClients = list()

        ## 初始化状态管理
        # self.current_poseRobotA = [0.0] * 6  ## 六轴 J1-J6 
        # self.current_poseRobotB = [0.0] * 4  ## 四轴 J1-J4
        self.current_poseRobotA = [0.0,0.0,0.0,0.0,0.0,0.0,198.67,0.0,230.72,0.0,0.0,0.0,0.0]
        self.current_poseRobotB = [0.0,0.0,0.0,0.0,0.0,0.0,198.67,0.0,230.72,0.0,0.0,0.0,0.0]
        self.target_poseA = [0.0] * 13   ## 示教点位
        self.target_poseB = [0.0] * 13   ## 示教点位
        
        self.speed = 100.0  # mm/s
        self.robotA_status = "Idle"
        self.robotB_status = "Idle"

        self.bo_out =0x00 

        ## 初始化时预填充配置值
        for i in range(6):
            CL.set_value(f'j{i+1}', self.current_poseRobotA[i])

        for i in range(6):
            CL.set_value(f'c{i+1}', self.current_poseRobotA[i+6])
        
        for i in range(4):
            CL.set_value(f'j{i + 7}', self.current_poseRobotB[i])

        for i in range(4):
            CL.set_value(f'c{i + 7}', self.current_poseRobotB[i+6])

        self.host = socket.gethostname()
        self._master_init()
        
        self.lock = threading.Lock()
        self.ioa_lock = threading.Lock()  # 新增IOA专用锁

        ## 路径规划
        self.planner = LinearPlanner()
        self.trajectoryRobotA = Queue()
        self.trajectoryRobotB = Queue()

        self._running = True

        # self.monitorRegisterControl = threading.Thread(target=self._run)
        # self.monitorRegisterControl.start()

        self.ioa_server = IOAServer(self)
        self.algo_th = threading.Thread(target=self.ioa_server.run)
        self.algo_th.start()

        self.condition = threading.Condition()
        self.movement_complete = False

        ## 后台服务
        self._update_thread = threading.Thread(target=self._state_updater)
        self._update_thread.start()

        time.sleep(1)

        self.th_update = Thread(target= self.status_update)
        self.th_update.start()


    def _master_init(self):
        self.master_ = modbus_tcp.TcpMaster(host=self.host)

    def monitor_server_socket(self):
        # 用于追踪已连接的客户端地址
        connected_clients = set()
        while True:
            ## 过滤掉无效的套接字

            valid_sockets = [s for s in self.server._sockets if s.fileno() >= 0]  
            try:
                inputready, _, _ = select.select(valid_sockets, [], [], 1.0)
                for sock in inputready:
                    if sock == self.server._sock: 
                        try:
                            client, address = sock.accept()
                            client.setblocking(0)
                            fileno = client.fileno()
                            addr_str = str(address)

                            ## 如果是新客户端，则打印连接信息并添加到已连接客户端集合中
                            if addr_str not in connected_clients:
                                connected_clients.add(addr_str)
                                self.modbusClients.append(address) 
                                self.server._sockets.append(client)

                        except OSError as e:
                            ## 忽略特定的非阻塞操作错误 [Errno 11] Resource temporarily unavailable 或 [Errno 35] Resource temporarily unavailable
                            if e.errno != errno.EAGAIN and e.errno != errno.EWOULDBLOCK and e.errno != errno.EINPROGRESS:
                                print(f"Unexpected exception occurred during accept(): {e}")
                                raise
            
            except select.error as e:
                ## 处理 select 错误...
                print(f"Select error occurred: {e}")


    def modbus_tcp_init(self):
        try:
            self.server = modbus_tcp.TcpServer(port=self.port,address=self.host,
                                               timeout_in_sec=5)
            self.server.start()
            self.register_init()
        
        except Exception as e:
            robot_logger.error(e,exc_info=True)

    def register_init(self,):
        self.slave =self.server.add_slave(1) 
        self.slave.add_block("robot6",cst.HOLDING_REGISTERS, 0, 600) 
        self.slave.add_block("var", cst.COILS, 0, 600)
    
    def int2float(self,a,b):
        f=0
        try:
            z0=hex(a)[2:].zfill(4) #取0x后边的部分 右对齐 左补零
            z1=hex(b)[2:].zfill(4) #取0x后边的部分 右对齐 左补零
            z=z0+z1 #高字节在前 低字节在后
            f=struct.unpack('!f', bytes.fromhex(z))[0] #返回浮点数
        
        except BaseException as e:
            print(e)
        return f

    def float_to_int16s(self,f):
        # 将浮点数打包成32位二进制数据
        b = struct.pack('f', f)
        # 将32位二进制数据拆分成两个16位二进制数据
        i1, i2 = struct.unpack('HH', b)
        return i2, i1

    def status_update(self,):
        while True:
            try:
                time.sleep(0.005)
                ### 更新机器人的状态
                status = []
                DI_status = []
                DO_status = []

                
                s = self.robotA_status
                if s != None:
                    if s == "Idle":
                        status.append(1)
                    elif s== "Stopped":
                        status.append(2)
                    elif s== "Home":
                        status.append(3)
                    elif s == "Run":
                        status.append(4)
                    else:
                        status.append(0)
                    
                    for i in range(1, 7):
                        a = self.float_to_int16s(CL.get_value(f"j{i}"))
                        status.append(a[0])
                        status.append(a[1])


                    for i in range(1, 7):
                        # ac = CL.get_value(f"c{i}")
                        # print(f"--{ac}")
                        b = self.float_to_int16s(CL.get_value(f"c{i}"))
                        status.append(b[0])
                        status.append(b[1])
                        # if i ==6:
                        #     slider_status.append(a[0])
                        #     slider_status.append(a[1])
                    
                    ### 增加机器人的类型
                    status.append(1)     
                    ### 将机器人的状态更新到0号寄存器
                    self.slave.set_values("robot6", 0, status)
                    # self.modbus_master.write_multiple_registers(1, 0, status)
                    # CL.set_value("BO0",self.bo_out)

                    DO_status = self.float_to_int16s(CL.get_value("BO0"))
                    self.slave.set_values("robot6", 28, DO_status)
                    
            
                # ### 更新机器人2的状态
                status_2 = []
                s_ = self.robotB_status
                if s_ != None:
                    if s_ == "Idle":
                        status_2.append(1)
                    elif s_== "Stopped":
                        status_2.append(2)
                    elif s_== "Home":
                        status_2.append(3)
                    elif s_ == "Run":
                        status_2.append(4)
                    else:
                        status_2.append(0)
                
                for i in range(7, 11):
                    a = self.float_to_int16s(CL.get_value(f"j{i}"))
                    status_2.append(a[0])
                    status_2.append(a[1])
                
                b = self.float_to_int16s(0)
                status_2.append(b[0])
                status_2.append(b[1])
                c = self.float_to_int16s(0)
                status_2.append(c[0])
                status_2.append(c[1])

                for i in range(7, 11):
                    # ac = CL.get_value(f"c{i}")
                    # print(f"--{ac}")
                    b = self.float_to_int16s(CL.get_value(f"c{i}"))
                    status_2.append(b[0])
                    status_2.append(b[1])

                b1 = self.float_to_int16s(0)
                status_2.append(b1[0])
                status_2.append(b1[1])
                c1 = self.float_to_int16s(0)
                status_2.append(c1[0])
                status_2.append(c1[1])

                ### 增加机器人的类型
                status.append(2) 
                self.slave.set_values("robot6", 60, status_2)
                # slider_status_2 = []
                # s_2 = self.status
                # if s_2 != None:
                #     if s_2["state"] == "Idle":
                #         status_2.append(1)
                #     elif s_2["state"] == "Stop":
                #         status_2.append(2)
                #     elif s_2["state"] == "Home":
                #         status_2.append(3)
                #     elif s_2["state"] == "Run":
                #         status_2.append(4)
                #     else:
                #         status_2.append(0)
                #     ang = s_2["angle"]
                #     for i in range(len(ang)):
                #         a = self.float_to_int16s(ang[i])
                #         status_2.append(a[0])
                #         status_2.append(a[1])

                #     coor = s_2["coordinate"]
                #     for i in range(len(coor)):
                #         a = self.float_to_int16s(coor[i])
                #         status_2.append(a[0])
                #         status_2.append(a[1])
                #         if i ==6:
                #             slider_status_2.append(a[0])
                #             slider_status_2.append(a[1])
                    
                #     ### 增加机器人的类型
                #     status_2.append(self.vsrserver.robot_control.robot_type_number_2)     
                #     # ### 增加数字仿真服务状态
                #     # status_2.append(self.vsrserver.connect_flag)
                #     ### IO状态
                #     ### 将2号机器人的状态更新到100号寄存器
                #     # self.slave.set_values("robot6", 60, status_2)
                #     self.modbus_master.write_multiple_registers(1, 60, status_2)

            except Exception as e:
                robot_logger.error(e,exc_info=True)


    def connect(self):
        try:
            if self.simulate:
                self._init_simulator()
            else:
                self._connect_hardware()
            
            # 确保IOA服务在连接后启动
            if not hasattr(self, 'ioa_server'):
                self.ioa_server = IOAServer(self)
                self.algo_th = threading.Thread(target=self.ioa_server.run)
                self.algo_th.start()
            
            self._running = True
            logging.info(f"Controller started in {'SIMULATION' if self.simulate else 'REAL'} mode")
        
        except Exception as e:
            self.stop()
            raise RobotNotConnectedError(f"连接失败: {str(e)}")

    def stop(self):
        self._running = False
        self.robotA_status = "STOPPED"
        self.robotB_status = "STOPPED"
        
        # 停止IOA服务并等待线程退出
        if hasattr(self, 'ioa_server'):
            self.ioa_server.stop()
        
        # 确保更新线程终止
        if self._update_thread.is_alive():
            self._update_thread.join(timeout=1)
            if self._update_thread.is_alive():
                robot_logger.warning("状态更新线程未正常退出")
        
        # 确保IOA服务线程终止
        if hasattr(self, 'algo_th') and self.algo_th.is_alive():
            self.algo_th.join(timeout=1)
            if self.algo_th.is_alive():
                robot_logger.warning("IOA服务线程未正常退出")
        
        logging.info("Controller stopped gracefully.")

    ### 机械臂相关指令
    ## 设置DO状态
    def SetDO(self, do, state):
        try:
            do_map = {"0": 0, "1": 1, "2": 2, "3": 3, "4": 4, "5":5, "6":6, "7":7}
            do_new = str(do).strip()
            if do_new in do_map:
                with self.ioa_lock:
                    add = do_map[do_new]
                    new_bo = self.bo_count(self.bo_out, add, value=state)
                    self.bo_out = new_bo
                    ## 同步更新到ioa数据发送
                    CL.set_value("BO0", new_bo)  

        except Exception as e:
            print(f"RobotLua class SetDO error is {e}")

    def bo_count(self, BO, bo_num, value):
        # BO 是一个8位的16进制数
        # bo_bit 是第几个IO
        # value 是DO的值
        # 将DO状态上传IOA服务器

        try:
            bit_num = 0b00000000
            if value == 0:
                if bo_num == 0:
                    bit_num = 0b11111110
                elif bo_num == 1:
                    bit_num = 0b11111101
                elif bo_num == 2:
                    bit_num = 0b11111011
                elif bo_num == 3:
                    bit_num = 0b11110111
                elif bo_num == 4:
                    bit_num = 0b11101111
                elif bo_num == 5:
                    bit_num = 0b11011111
                elif bo_num == 6:
                    bit_num = 0b10111111
                elif bo_num == 7:
                    bit_num = 0b01111111
                BO = BO & bit_num
            elif value == 1:
                if bo_num == 0:
                    bit_num = 0b00000001
                elif bo_num == 1:
                    bit_num = 0b00000010
                elif bo_num == 2:
                    bit_num = 0b00000100
                elif bo_num == 3:
                    bit_num = 0b00001000
                elif bo_num == 4:
                    bit_num = 0b00010000
                elif bo_num == 5:
                    bit_num = 0b00100000
                elif bo_num == 6:
                    bit_num = 0b01000000
                elif bo_num == 7:
                    bit_num = 0b10000000
                BO = BO | bit_num
            return BO

        except Exception as e:
            print(e)

    def set_joint_angle(self, com_Pose = None,  speed=None, com_Num = None):
        if not self._running:
            raise RobotNotConnectedError("Controller not connected")

        if speed:
            self.speed = speed

        # print(f"set_joint_angle {com_Pose[0]}")
        try:
            with self.lock:
                self.robotA_status = "PLANNING"
                self.robotB_status = "PLANNING"
                if com_Num == 1:
                    path = self.planner.generate(

                        start = self.current_poseRobotA,
                        # end = com_Pose[:6],
                        end= com_Pose,
                        speed = self.speed
                    )
                elif com_Num == 2:
                    path = self.planner.generate(
                        start= self.current_poseRobotB,
                        # end = com_Pose[:4],
                        end= com_Pose,
                        speed = self.speed
                    )
                    
                self.trajectoryRobotA.queue.clear()
                self.trajectoryRobotB.queue.clear()

                for point in path:

                    point_list = point.tolist() if hasattr(point, 'tolist') else list(point)
                    if com_Num == 1:
                        self.trajectoryRobotA.put(point_list)
                        self.target_poseA = point_list.copy() 
                        self.robotA_status = "Run" 
                    elif com_Num == 2:
                        self.trajectoryRobotB.put(point_list)
                        self.target_poseB = point_list.copy()
                        self.robotB_status = "Run" 

                ## 重置完成标志
                with self.condition:
                    self.movement_complete = False

                ## 等待运动完成
                with self.condition:
                    while not self.movement_complete:
                        # print(111111)
                        # time.sleep(0.1)
                        self.condition.wait()

        except Exception as e:
            raise MotionPlanningError(f"Path planning failed: {str(e)}")
    
    ## 机械臂停止
    def robot_stop(self, com_Num=None):
        if not self._running:
            raise RobotNotConnectedError("Controller not connected")

        with self.lock:
            # 清空对应机械臂的轨迹队列
            if com_Num == 1:
                self.trajectoryRobotA.queue.clear()
                self.robotA_status = "Stop"
            elif com_Num == 2:
                self.trajectoryRobotB.queue.clear()
                self.robotB_status = "Stop"
            else:  # 未指定时停止所有机械臂
                self.trajectoryRobotA.queue.clear()
                self.trajectoryRobotB.queue.clear()

            # 唤醒所有等待线程并标记运动完成
            with self.condition:
                self.movement_complete = True
                self.condition.notify_all()

    ## 机械臂复位（回零）
    def robot_home(self, speed = None, com_Num= None):
        if not self._running:
            raise RobotNotConnectedError("Controller not connected")

        if speed:
            self.speed = speed

        com_Pose = [0.0,0.0,0.0,0.0,0.0,0.0,198.67,0.0,230.72,0.0,0.0,0.0,0.0]
        try:
            with self.lock:
                if com_Num == 1:
                    path = self.planner.generate(

                        start = self.current_poseRobotA,
                        end = com_Pose,
                        speed = self.speed
                    )
                elif com_Num == 2:
                    path = self.planner.generate(
                        start= self.current_poseRobotB,
                        end = com_Pose,
                        speed = self.speed
                    )
                    
                self.trajectoryRobotA.queue.clear()
                self.trajectoryRobotB.queue.clear()

                for point in path:
                    if com_Num == 1:
                        self.trajectoryRobotA.put(point)
                        self.target_poseA = com_Pose
                        self.robotA_status = "Home"
                    elif com_Num == 2:
                        self.trajectoryRobotB.put(point)
                        self.target_poseB = com_Pose
                        self.robotB_status = "Home"
                
                # 重置完成标志
                with self.condition:
                    self.movement_complete = False

                # 等待运动完成
                with self.condition:
                    while not self.movement_complete:
                        time.sleep(0.5)
                        self.condition.wait()
            
        except Exception as e:
            raise MotionPlanningError(f"Path planning failed: {str(e)}")

    def get_robot_type(self,type_value):

        if type_value == 1:
            return "KT-R6"  
        elif type_value == 2:
            return "KT-R4"
        else:
            return "未连接" 



    # 比较时增加容差判断
    def is_position_equal(self, current, target):
        return all(abs(a - b) < POSITION_THRESHOLD for a, b in zip(current, target))

    ## 机械臂状态更新（核心代码）
    def _state_updater(self):
        while self._running:
            # 更新RobotA的轨迹
            try:
                temp_pose = self.trajectoryRobotA.get_nowait()
                self.current_poseRobotA = list(temp_pose)
                # 更新数据库中的关节值
                CL.set_value('j1', self.current_poseRobotA[0])
                CL.set_value('j2', self.current_poseRobotA[1])
                CL.set_value('j3', self.current_poseRobotA[2])
                CL.set_value('j4', self.current_poseRobotA[3])
                CL.set_value('j5', self.current_poseRobotA[4])
                CL.set_value('j6', self.current_poseRobotA[5])

                CL.set_value('c1', self.current_poseRobotA[6])
                CL.set_value('c2', self.current_poseRobotA[7])
                CL.set_value('c3', self.current_poseRobotA[8])
                CL.set_value('c4', self.current_poseRobotA[9])
                CL.set_value('c5', self.current_poseRobotA[10])
                CL.set_value('c6', self.current_poseRobotA[11])
            except queue.Empty:
                pass  # RobotA队列为空时不处理
            
            # 更新RobotB的轨迹
            try:
                temp_pose2 = self.trajectoryRobotB.get_nowait()
                self.current_poseRobotB = list(temp_pose2)
                # 更新数据库中的关节值
                CL.set_value('j7', self.current_poseRobotB[0])
                CL.set_value('j8', self.current_poseRobotB[1])
                CL.set_value('j9', self.current_poseRobotB[2])
                CL.set_value('j10', self.current_poseRobotB[3])

                CL.set_value('c7', self.current_poseRobotB[6])
                CL.set_value('c8', self.current_poseRobotB[7])
                CL.set_value('c9', self.current_poseRobotB[8])
                CL.set_value('c10', self.current_poseRobotB[9])
            except queue.Empty:
                pass  # RobotB队列为空时不处理
            
            # # 打印当前状态
            # print(f"current_poseRobotA: {self.current_poseRobotA}")
            # print(f"current_poseRobotB: {self.current_poseRobotB}")
            # print(f"target_pose: {self.target_poseA}")

            # 判断是否到达目标位置（比较全部有效关节）
            if len(self.target_poseA) >= 6 and self.is_position_equal(
                self.current_poseRobotA[:6],   # 实际关节角度
                self.target_poseA[:6]          # 目标关节角度
            ) and self.robotA_status in ["Run", "Home"] :
                with self.condition:
                    # print(222)
                    self.movement_complete = True
                    self.condition.notify_all()

            if len(self.target_poseB) >= 4 and self.is_position_equal(
                self.current_poseRobotB[:4],   # 实际关节角度
                self.target_poseB[:4]          # 目标关节角度
            ) and self.robotB_status in ["Run", "Home"] :
                with self.condition:
                    self.movement_complete = True
                    self.condition.notify_all()

            # target_a = list(self.target_poseA[:12])
            # target_b = list(self.target_poseB[:12])
            # if (self.current_poseRobotA[:12] == target_a) or (self.current_poseRobotB[:12] == target_b):
            #     print(22222222)
            #     with self.condition:
            #         self.movement_complete = True
            #         self.condition.notify_all()
                    
            time.sleep(0.1)  # 适当调整更新频率

    def _init_simulator(self):
        self.current_poseRobotA = [0.0] * 13
        self.current_poseRobotB = [0.0] * 13
        logging.info("Simulator initialized")

    def _connect_hardware(self):
        raise NotImplementedError("Hardware connection not implemented")

    # def __del__(self):
    #     self._running = False
    #     self.tcp.stop()
    #     if self._update_thread.is_alive():
    #         self._update_thread.join(timeout=5)

    def _run(self):
        logging.basicConfig(level=logging.DEBUG)
      
        try:
            self.connect()
            # 保持主线程运行
            # self.__init__()
            while True:
                time.sleep(1)
        
        except KeyboardInterrupt:
            self.stop()
            logging.info("Service stopped gracefully.")

###tcp向IOA发送数据包

class IOAServer:
    
    def __init__(self, controller):
        self.controller = controller
        self.tcp_server_socket = None
        self.address2 = ("0.0.0.0", 7070)
        self.connect_flag = 0
        # self.tcpServer()
        self._stop_event = threading.Event()

        self.connected_clients = []
        
        CL.set_value("BO0", 0)
        CL.set_value("BO1", 0)
        CL.set_value("FO[0]", self.float2bytes(75.0))
        CL.set_value("FO[1]", self.float2bytes(75.0))

    def tcpServer(self):
        self.tcp_server_socket = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
        self.tcp_server_socket.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEADDR, 1)
        self.tcp_server_socket.bind(self.address2)
        self.tcp_server_socket.listen(10)
        self.tcp_server_socket.settimeout(1) 

    ## 将浮点数打包成32位2进制数据
    def float2bytes(self,f):
        b=struct.pack('<f',f)        
        return b[0],b[1],b[2],b[3]  

    ## 检查总位数
    def checksum(self, Data, lenth):
        checksum_value=0
        data=list()
        for i in range(lenth-2):   #######从Data[2]开始，相应总长度减2
            data.append(Data[i+2])    
        for i in range(len(data)):
            checksum_value=((checksum_value+data[i]))%0xFFFF
        checksum_value_low=(checksum_value&0xFF)
        return checksum_value_low

    ## 打包发送数据--格式
    def set_sendData2IOA(self,):
        try:
            data = list()
            data.extend([0xff, 0xaa])
            data.append(0x01)
            data.extend([0x63, 0x00])
            data.extend([0x41, 0x00])
            data.extend([0x01, 0x05])

            # 子帧1: BI数据
            data.append(0x01)               #09
            data.extend([0x02,0x00])        #10,11
            data.extend([CL.get_value('BI0'),CL.get_value('BI1')])  #12,13

            # 子帧2: BO数据
            data.append(0x02)                           #14
            data.extend([0x02,0x00])                      #15,16       
            data.append(CL.get_value("BO0"))        #17
            # data.append(0xff)  
            data.append(CL.get_value("BO1"))        #18

            ###子帧（3）6个MS，float数据，来自ABB###
            data.append(0x03)                           #19
            data.extend([0x28,0x00])                    #20,21
            # with config_lock:

            if CL.get_value("j1") is not None:
                j0 = CL.get_value("j1")
                j1 = CL.get_value("j2")
                j2 = CL.get_value("j3")
                j3 = CL.get_value("j4")
                j4 = CL.get_value("j5")
                j5 = CL.get_value("j6")
            else:
                j0 = 0.0
                j1 = 0.0
                j2 = 0.0
                j3 = 0.0
                j4 = 0.0
                j5 = 0.0

            if CL.get_value("j7") is not None:
                j6 = CL.get_value("j7")
                j7 = CL.get_value("j8")
                j8 = CL.get_value("j9")
                j9 = CL.get_value("j10")
            else:
                    j6 = 0.0
                    j7 = 0.0
                    j8 = 0.0
                    j9 = 0.0

            # print(j0, j1, j2, j3, j4, j5, j6,j7,j8, j9)
            # BO = CL.get_value("BO0")
            # print(f"BO0 {BO}")

            data.extend(self.float2bytes(j0))         #22-25
            data.extend(self.float2bytes(j1))         #26-29
            data.extend(self.float2bytes(j2))        #30-33
            data.extend(self.float2bytes(j3))         #34-37
            data.extend(self.float2bytes(j4))         #38-41
            data.extend(self.float2bytes(j5))        #42-45  
            
            data.extend(self.float2bytes(j6))
            data.extend(self.float2bytes(j7))         #34-37
            data.extend(self.float2bytes(j8))        #38-41
            data.extend(self.float2bytes(j9))  
            
            # 子帧4: FI数据
            data.append(0x84)                           #46
            data.extend([0x10,0x00])                    #47,48
            data.extend(CL.get_value("FI[0]"))      #49-52
            data.extend(CL.get_value("FI[1]"))      #53-56
            data.extend(CL.get_value("FI[2]"))      #57-60
            data.extend(CL.get_value("FI[3]"))      #61-64
            
            data.append(0x85)                           #65
            data.extend([0x10,0x00])     
            data.extend(CL.get_value("FO[0]"))      #FO 0
            data.extend(CL.get_value("FO[1]"))      #FO 0
            data_FO=[0x00]*8

            data.extend(data_FO)

            # 计算校验和
            data.append(self.checksum(data, 100))
            data.extend([0xff, 0x55])
            # print(data)
            return data
        
        except Exception as e:
            robot_logger.error(f"Data packing error: {str(e)}")
            return None

    def calculate_recvdata(self,recv_value,lenth):
        
        data=list()                       
        for i in range(0,lenth):
            data.append(recv_value[i])
        # print('接收到IOA数据')
        CL.set_value("BI0",data[12])    ###16位BI数据中低8位BI0----BI7                    
        CL.set_value("BI1",data[13])    ###16位BI数据中高8位BI8----BI15
        ###4个一组为一个FI，共4个FI,data[17]-data[32]
        tf0=(data[17:21])
        tf1=(data[21:25])
        tf2=(data[25:29])
        tf3=(data[29:33])
        CL.set_value("FI[0]",value=tf0)
        CL.set_value("FI[1]",value=tf1) ##FI.append(data[17+4*i:21+4*i])
        CL.set_value("FI[2]",value=tf2)
        CL.set_value("FI[3]",value=tf3)  

    def stop(self):
        self._stop_event.set()  # 触发停止事件
        if self.tcp_server_socket:
            try:
                self.tcp_server_socket.close()
            
            except Exception as e:
                robot_logger.error(f"关闭服务端套接字错误: {str(e)}")
            self.tcp_server_socket = None
        
        for client in self.connected_clients:
            try:
                client.close()
            
            except Exception as e:
                robot_logger.error(f"关闭客户端套接字错误: {str(e)}")
        self.connected_clients.clear()

    def run(self):
        while not self._stop_event.is_set():
            try:
                if not self.tcp_server_socket:
                    self.tcpServer()  # 确保套接字初始化
                
                try:
                    self.connect_flag = 0
                    self.client_socket, addr = self.tcp_server_socket.accept()
                    print("from:",addr)
                    time.sleep(0.1)
                    self.client_socket.settimeout(1)
                    self.connected_clients.append(self.client_socket)
                    self.connect_flag = 1
                    while not self._stop_event.is_set():
                        try:
                            recv_data=self.client_socket.recv(1024)
                            if len(recv_data)<36:  ##############处理接收数据
                                print(f"recv_data less then 36")
                                # client_socket,client_Addr=self.tcp_server_socket.accept()
                                # pass
                                break
                            else:
                                for i in range(0,len(recv_data)):
                                    if (recv_data[i]==0xff and recv_data[i+1]==0xaa):
                                        recv_data=recv_data[i:]
                                        break                  
                                if (recv_data[0]==0xff and recv_data[1]==0xaa):
                                    if (recv_data[8]==0x02):
                                        if (recv_data[9]==0x01 and recv_data[10]==0x02 and recv_data[14]==0x84 and recv_data[15]==0x10):
                                            self.calculate_recvdata(recv_data,36)
                                            senddata=self.set_sendData2IOA()
                                            send_data=bytes(senddata)
                                            # print(f"send to IOA data {send_data}")                                   
                                            self.client_socket.send(send_data)                                                               
                                        else:
                                            print("congfig参数不对")
                                    else:
                                        print("外设数目不为2")                            
                                else:
                                    print('报头校验不通过')
                        
                        except Exception as e:
                            robot_logger.error(f"Accept error: {str(e)}")

                    self.client_socket.close()
                    self.connected_clients.remove(self.client_socket)

                except socket.timeout:
                    continue
                except Exception as e:
                    if not self._stop_event.is_set():
                        robot_logger.error(f"accept connect failed {str(e)}")
                    break

            except Exception as e:
                if not self._stop_event.is_set():
                    robot_logger.error(f"Accept2 error: {str(e)}")



#### 此版本不再执行main，仅引用Controler_Server类,执行到mainwindow.py中
if __name__ == '__main__':

    process_master = process_Master()
    # rtu_master = rtu_Master()
    server = VSRControler_Server()
    server.dorun()



